﻿#include "utility.h"

/**
 * @brief Utility::calcDistance 计算二维平面两点之间的距离
 * @param start 起始点的坐标
 * @param end 目标点的坐标
 * @return 距离
 */
double Utility::calcDistance(const Point& start, const Point& end) { return sqrt(pow((end.x() - start.x()), 2) + pow((end.y() - start.y()), 2)); }

/**
 * @brief Utility::calcRadian 计算两点连线的弧度,注意，并不使用两点的弧度进行计算
 * @param start 起始点的坐标
 * @param end 终点的坐标
 * @return 两点连线的弧度
 */
double Utility::calcRadian(const Point& start, const Point& end) { return atan2((end.y() - start.y()), (end.x() - start.x())); }

/**
 * @brief Utility::calcGamma 把弧度差和距离差融合,详见论文<<轮式移动机器人轨迹跟踪控制算法的研究>>尉成果，赖 欢，王太江
 * @param cur 当前点的坐标
 * @param start 起点的坐标
 * @param end 终点的坐标
 * @param isReverse 向前走还是向后走,默认是向前走
 * @return 弧度差和距离差融合值gamma
 */
double Utility::calcGamma(const Point& cur, const Point& start, const Point& end, bool isForward, double r) {
  double radian = calcRadian(start, end);
  double h = (cur.x() - start.x()) * sin(radian) - (cur.y() - start.y()) * cos(radian);
  double alpha = asin(h / r);

  //    if (fabs(h) >= fabs(r))
  //    {
  //        alpha = 0;
  //    }

  double gamma;
  //如果是后退的话,那么当前位置的方向需要取反,即当前弧度减去180度
  //注意:连续的弧度加减需要连续的弧度纠正,不能直接一条弧度纠正
  if (isForward) {
    gamma = cur.radian() - alpha;
    radianCorrection(&gamma);
    gamma -= radian;
  } else {
    gamma = cur.radian() - M_PI;
    radianCorrection(&gamma);
    gamma -= alpha;
    radianCorrection(&gamma);
    gamma -= radian;
  }
  radianCorrection(&gamma);
  // qDebug() << "r:" << r << " h:" << h << " asin(h/r):" << asin(h / r) << " radian:" << radian << " gamma:" << gamma;
  return gamma;
}

double Utility::calcPointToLineDistance(const Point cur, const Point start, const Point end) {
  double radian = calcRadian(start, end);

  return (cur.x() - start.x()) * sin(radian) - (cur.y() - start.y()) * cos(radian);
}

double Utility::calcRadErrStart2Target(const Point cur, const Point start, const Point end, const bool isforward) {
  double m_rad_start2target = calcRadian(start, end);
  double m_rad_err_start2target;
  if (isforward) {
    m_rad_err_start2target = cur.radian() - m_rad_start2target;
    Utility::radianCorrection(&m_rad_err_start2target);
  } else {
    m_rad_err_start2target = cur.radian() - M_PI;
    Utility::radianCorrection(&m_rad_err_start2target);
    m_rad_err_start2target -= m_rad_start2target;
    Utility::radianCorrection(&m_rad_err_start2target);
  }
  return m_rad_err_start2target;
}

Point Utility::calcShowPoint(const Point cur, const Point start, const Point end) {
  double m_rad_start2target = Utility::calcRadian(start, end);
  double m_rad_cur2target = Utility::calcRadian(cur, end);
  // double m_dis_start2target = Utility::calcDistance( start, end );
  // double m_dis_start2cur    = Utility::calcDistance( start, cur );
  double m_dis_cur2target = Utility::calcDistance(cur, end);

  //计算shadow point
  double rad_err = m_rad_cur2target - m_rad_start2target;
  Utility::radianCorrection(&rad_err);
  double m_dis_shadow2target = cos(rad_err) * m_dis_cur2target;
  // double m_dis_start2shadow  = m_dis_start2target - m_dis_shadow2target;
  Point m_shadow_point;

  m_shadow_point.setX(end.x() - m_dis_shadow2target * cos(m_rad_start2target));
  m_shadow_point.setY(end.y() - m_dis_shadow2target * sin(m_rad_start2target));

  return m_shadow_point;
}

/**
 * @brief Utility::ascii2Num ascii转16进制,注意:当输入并非16进制数字,则返回-1
 * @param input
 * @return
 */
int Utility::ascii2Hex(char input) {
  if ((input >= 0x30) && (input <= 0x39)) {
    input -= 0x30;
  } else if ((input >= 0x41) && (input <= 0x46)) {
    input -= 0x37;
  } else if ((input >= 0x61) && (input <= 0x66)) {
    input -= 0x57;
  } else {
    input = -1;
  }
  return input;
}

/**
 * @brief Utility::delayMs 非阻塞式延时函数
 * @param mSec 延迟的毫秒数
 */
void Utility::delayMs(const int& mSec) {
  QEventLoop* loop = new QEventLoop();

  QTimer::singleShot(mSec, loop, SLOT(quit()));
  loop->exec();
}
/**
 * @brief Utility::calcDisShadow2target 计算投影点离终点的距离
 * @param
 */
double Utility::calcDisShadow2target(const Point& start, const Point& end, const Point& cur) {
  double m_rad_cur2target = Utility::calcRadian(cur, end);
  double m_rad_start2target = Utility::calcRadian(start, end);
  double m_dis_cur2target = Utility::calcDistance(cur, end);
  double rad_err = m_rad_cur2target - m_rad_start2target;
  Utility::radianCorrection(&rad_err);
  double m_dis_shadow2target = cos(rad_err) * m_dis_cur2target;
  return fabs(m_dis_shadow2target);
}

double Utility::calcDisPA2LBC(const Point& a, const Point& b, const Point& c) {
  double A = b.y() - c.y();
  double B = c.x() - b.x();
  double C = b.x() * c.y() - c.x() * b.y();

  return abs(A * a.x() + B * a.y() + C) / sqrt(A * A + B * B);
}

void Utility::blockDelayMs(int ms) {
  QElapsedTimer dieTime;

  dieTime.start();
  while (dieTime.elapsed() < ms) {}
}

/**
 * @brief Utility::BCC 异或校验，对传入的字节数组进行异或，得出校验值
 * @param order 传入的字节数组
 * @param len 需要校验的长度
 * @return 校验值
 */
char Utility::BCC(const QByteArray& order, int len) {
  char bcc = 0x00;

  for (int i = 0; i <= len; i++) { bcc ^= order.at(i); }
  return bcc;
}

/**
 * @brief Utility::BCC 异或校验，对传入的字节数组进行异或，得出校验值
 * @param order 传入的字节数组
 * @param start 起始的index
 * @param len 需要校验的长度
 * @return 校验值
 */
char Utility::BCC(const QByteArray& order, int start, int len) {
  char bcc = 0x00;

  for (int i = start; i < start + len; i++) { bcc ^= order.at(i); }
  return bcc;
}

/**
 * @brief Utility::CKS 和校验，对传入的字节数组进行相加，溢出部分去掉，只保留最低位字节
 * @param order 传入的字节数组
 * @param len 校验的长度
 * @return 校验值
 */
uchar Utility::CKS(const QByteArray& order, int len) {
  uchar result = 0x00;

  for (int i = 0; i < len; i++) { result += order.at(i); }
  return result;
}

/**
 * @brief Utility::CKS4TL740D 和校验，对传入的字节数组进行相加，溢出部分去掉，只保留最低位字节
 * @param order 传入的字节数组
 * @param start 开始的index
 * @param len 校验的长度
 * @return 校验值
 */
uchar Utility::CKS(const QByteArray& order, int start, int len) {
  uchar crc = 0;

  for (int i = start; i < start + len; i++) { crc += order.at(i); }
  return crc;
}

/**
 * @brief Utility::CRC16_ModBus CRC16位校验
 * @param order 传入的字节数组
 * @param num 开始的index
 * @return 校验值
 */
QByteArray Utility::CRC16_ModBus(const QByteArray& order, int num) {
  quint16 crcRegistor = 0xFFFF;
  int index = num;
  QByteArray crc(2, 0x00);

  while (num--) {
    crcRegistor ^= (uchar(order.at(index - num - 1)));
    //      printf("%x ", (uchar)order.at(index - num - 1));
    //      crcRegistor ^= (order.at(index-num-1) & 0xFF);
    for (qint8 i = 0; i < 8; i++) {
      if (crcRegistor & 1) {
        crcRegistor = (crcRegistor >> 1) ^ 0xA001;  //串口数据传输时低位在前，高位在后，8005应该倒过来读，就是A001
      } else {
        crcRegistor >>= 1;
      }
    }
  }
  //   qDebug() << QString::number(crcRegistor, 16);
  crc[0] = static_cast<char>(crcRegistor);
  crc[1] = static_cast<char>(crcRegistor >> 8);
  return crc;
}

QByteArray Utility::CRC16_X25(const QByteArray& order, int num) {
  quint16 crcRegistor = 0xFFFF;
  int index = num;
  QByteArray crc(2, 0x00);

  while (num--) {
    crcRegistor ^= (uchar(order.at(index - num - 1)));
    //           printf("%x ",(uchar)order.at(index-num-1));
    //        crcRegistor ^= (order.at(index-num-1) & 0xFF);
    for (qint8 i = 0; i < 8; i++) {
      if (crcRegistor & 1) {
        crcRegistor = (crcRegistor >> 1) ^ 0x8408;  //串口数据传输时低位在前，高位在后，8005应该倒过来读，就是A001
      } else {
        crcRegistor >>= 1;
      }
    }
  }

  crcRegistor ^= 0xFFFF;
  //       qDebug()<<QString::number(crcRegistor,16);
  crc[0] = static_cast<char>(crcRegistor);
  crc[1] = static_cast<char>(crcRegistor >> 8);
  return crc;
}

QString Utility::QbyteArry2Qstring(const QByteArray& data) {
  QString strDisplay;
  QString str = data.toHex().data();

  str = str.toUpper();
  for (int i = 0; i < str.length(); i += 2) {
    QString st = str.mid(i, 2);
    strDisplay += st;
    strDisplay += " ";
  }
  return strDisplay;
}

/**
 * @brief Utility::calcEncoderPosition 通过编码器增量计算编码器数据
 * @param unitLength 每个编码器脉冲对应的实际距离，unitLength = 2*PI*轮子半径/轮子走一圈的脉冲数
 * @param leftInc 左轮编码器增量
 * @param rightInc 右轮编码器增量
 * @param radian 当前弧度
 * @param encPose 传入的编码器坐标指针，函数会直接修改其坐标值
 */
void Utility::calcEncoderPosition(double unitLength, double leftInc, double rightInc, double radian, Point* encPose) {
  //左右轮行驶距离
  double l_dis = leftInc * unitLength;
  double r_dis = rightInc * unitLength;
  //距离增量
  double delta_dis = (l_dis + r_dis) / 2;

  //计算实时的坐标值与弧度值
  encPose->addX(delta_dis * cos(radian));
  encPose->addY(delta_dis * sin(radian));
  encPose->setRadian(radian);
}

/**
 * @brief Utility::calcEncoderPositionAndRad 通过编码器增量计算编码器数据
 * @param unitLength 每个编码器脉冲对应的实际距离，unitLength = 2*PI*轮子半径/轮子走一圈的脉冲数
 * @param base 两轮间距离的一半
 * @param leftInc 左轮编码器增量
 * @param rightInc 右轮编码器增量
 * @param encPose 传入的编码器坐标指针，函数会直接修改其坐标值
 */
void Utility::calcEncoderPositionAndRad(double unitLength, double agvRadius, int leftInc, int rightInc, Point* encPose) {
  //左右轮行驶距离
  double l_dis = static_cast<double>(leftInc * unitLength);
  double r_dis = static_cast<double>(rightInc * unitLength);
  //距离增量
  double delta_dis = (l_dis + r_dis) / 2;
  double delta_rad = (r_dis - l_dis) / (agvRadius * 2);

  //计算实时的坐标值与弧度值
  encPose->addX(delta_dis * cos(encPose->radian()));
  encPose->addY(delta_dis * sin(encPose->radian()));
  double rad = encPose->radian() + delta_rad;
  radianCorrection(&rad);
  encPose->setRadian(rad);
}

/**
 * @brief Utility::calcCanPosition 通过CAN反馈的实时速度计算坐标(用于预测坐标多)
 * @param dtime 该函数的执行时间 单位：ms
 * @param dleftSpeed 左轮速度  单位 m/s
 * @param drightSpeed 右轮速度 单位 m/s
 * @param radian 当前弧度
 * @param encPose 传入预测坐标指针，函数会直接计数出该速度下单位时间内的坐标变化
 */
void Utility::calcCanPosition(double dtime, double dleftSpeed, double drightSpeed, double radian, Point* encPose) {
  //左右轮行驶距离 l（mm）=速度（m/s）*[时间（ms）/1000] * 1000
  double l_dis = dleftSpeed * dtime;
  double r_dis = drightSpeed * dtime;
  //距离增量
  double delta_dis = (l_dis + r_dis) / 2;

  //计算实时的坐标值与弧度值
  encPose->addX(delta_dis * cos(radian));
  encPose->addY(delta_dis * sin(radian));
  encPose->setRadian(radian);
}

/**
 * @brief Utility::calcCanPositionAndRad 通过CAN反馈的实时速度计算坐标和弧度
 * @param dtime 该函数的执行时间 单位：ms
 * @param agvRadius 两轮间距离的一半
 * @param dleftSpeed 左轮速度  单位 m/s
 * @param drightSpeed 右轮速度 单位 m/s
 * @param encPose 传入的编码器坐标指针，函数会直接修改其坐标值
 */
void Utility::calcCanPositionAndRad(double dtime, double agvRadius, double dleftSpeed, double drightSpeed, Point* encPose) {
  //左右轮行驶距离 l（mm）=速度（m/s）*[时间（ms）/1000] * 1000
  double l_dis = dleftSpeed * dtime;
  double r_dis = drightSpeed * dtime;
  //距离增量
  double delta_dis = (l_dis + r_dis) / 2;
  double delta_rad = (r_dis - l_dis) / (agvRadius * 2);

  //计算实时的坐标值与弧度值
  encPose->addX(delta_dis * cos(encPose->radian()));
  encPose->addY(delta_dis * sin(encPose->radian()));
  double rad = encPose->radian() + delta_rad;
  radianCorrection(&rad);
  encPose->setRadian(rad);
}

void Utility::calcImuEncoderPosition(double unitLength, double leftInc, double rightInc, double radian, Point* encPose) {
  //左右轮行驶距离
  double l_dis = static_cast<double>(leftInc * unitLength);
  double r_dis = static_cast<double>(rightInc * unitLength);
  //距离增量
  double delta_dis = (l_dis + r_dis) / 2;
  double delta_rad = radian;

  //计算实时的坐标值与弧度值
  encPose->addX(delta_dis * cos(encPose->radian()));
  encPose->addY(delta_dis * sin(encPose->radian()));
  double rad = encPose->radian() + delta_rad;
  radianCorrection(&rad);
  encPose->setRadian(rad);
}

/**
 * @brief Utility::radianCorrection 弧度校正，使弧度在(-PI,PI]区间.
 *          注意:连续的弧度加减需要连续的弧度纠正,不能连续加减后直接一条弧度纠正
 * @param radian 传入的弧度数据的指针，函数会直接修改其值
 */
void Utility::radianCorrection(double* radian) {
  if ((*radian > M_PI) && (*radian <= 3 * M_PI))  //(PI,3PI]
  {
    *radian -= 2 * M_PI;
  } else if ((*radian <= -M_PI) && (*radian >= -3 * M_PI))  //[-3PI,-PI]
  {
    *radian += 2 * M_PI;
  }
}

Utility::Utility() = default;
